package pl.edu.agh.pros.android.ros;

import org.ros.message.MessageListener;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Subscriber;

import pl.edy.agh.pros.adk.util.ADKSendUtility;
import pl.edy.agh.pros.adk.util.KoalaCommands;
import pl.edy.agh.pros.adk.util.SpeedPreprocessor;

public class SensorListener extends AbstractNodeMain {

	private final ADKSendUtility sendUtility;

	public SensorListener(ADKSendUtility sendUtility) {
		this.sendUtility = sendUtility;
	}

	@Override
	public GraphName getDefaultNodeName() {
		return new GraphName("pros_camera/sensor_listener");
	}

	@Override
	public void onStart(ConnectedNode connectedNode) {
		Subscriber<std_msgs.String> subscriber = connectedNode.newSubscriber("/koala/sensors", std_msgs.String._TYPE);
		subscriber.addMessageListener(new MessageListener<std_msgs.String>() {
			@Override
			public void onNewMessage(std_msgs.String message) {
				updateRobotSpeed(message.getData());
			}
		});
	}

	private void updateRobotSpeed(String speed) {
		String[] values = speed.split(",");
		if (values.length < 2) {
			return;
		}
		byte x = Byte.parseByte(values[0]);
		byte y = Byte.parseByte(values[1]);
		sendUtility.sendMessage(KoalaCommands.TYPE_SPEED, SpeedPreprocessor.process(x, y));
	}
}
